'''
Author: wangyingjie 2778809626@qq.com
Date: 2025-07-03 15:35:52
LastEditors: wangyingjie 2778809626@qq.com
LastEditTime: 2025-07-06 13:26:37
FilePath: /two_wheeled_car/src/bot_nav/launch/path_planning.launch.py
Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
'''
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    """
    生成用于启动导航堆栈的LaunchDescription。

    该函数通过读取配置文件来初始化导航堆栈中的各个服务器和节点。
    它们包括控制器服务器、规划器服务器、行为服务器、航点跟随器节点和行为树导航器。
    此外，生命周期管理器用于管理这些节点的生命周期。

    Returns:
        LaunchDescription: 包含所有导航相关节点的LaunchDescription。
    """
    # 获取导航配置文件的路径
	# controller.yaml             控制机器人运动控制逻辑
	# planner_server.yaml         路径规划算法配置
	# behavior_tree.yaml          定义恢复行为、避障等逻辑
	# bt_navigator.yaml           行为树导航策略配置
	# waypoint_follower.yaml      执行航点任务相关配置
    controller_yaml = os.path.join(get_package_share_directory('bot_nav'),'config','controller.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('bot_nav'),'config','bt_navigator.yaml')
    planner_yaml = os.path.join(get_package_share_directory('bot_nav'), 'config','planner_server.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('bot_nav'), 'config', 'recovery.yaml')
    wp_follower = os.path.join(get_package_share_directory('bot_nav'), 'config', 'waypoints.yaml')

    # 返回包含导航堆栈节点的LaunchDescription
    return LaunchDescription([
        # 控制器服务器节点，负责执行导航指令
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_yaml]),
        
        # 规划器服务器节点，负责路径规划
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]),
        
        # 行为服务器节点，负责处理恢复行为
        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='behavior_server',
            parameters=[recovery_yaml],
            output='screen'),
        
        # 航点跟随器节点，用于跟随预定义的航点
        Node(
            package='nav2_waypoint_follower',
            executable='waypoint_follower',
            name='waypoint_follower_node',
            output='screen',
            parameters=[wp_follower]),
        
        # 行为树导航器节点，用于基于行为树的导航
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]),
 
        # 生命周期管理器节点，用于管理其他导航节点的生命周期
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_pathplanner',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['planner_server', 
                                        'controller_server',
                                        'behavior_server',
                                        'waypoint_follower_node',
                                        'bt_navigator'
                                        ]}])
    ])